from machine import SPI, Pin
import utime
import urandom
import math
from machine import I2C
import st7789
import icm20948
from ulab import numpy as np
import nfc
nfc.init()
spi = SPI(0, baudrate=40000000, polarity=1, phase=0, bits=8, endia=0, sck=Pin(6), mosi=Pin(8))
display = st7789.ST7789(spi, 240, 240, reset=Pin(11,func=Pin.GPIO, dir=Pin.OUT), dc=Pin(7,func=Pin.GPIO, dir=Pin.OUT))
display.init()
imu = icm20948.ICM20948(I2C(0), dmp=True)
utime.sleep(1)
backgroud_clr = st7789.color565(87, 182, 177)
obj_clr = st7789.color565(255, 255, 255)

loop_clr=[st7789.color565(255, 255, 255)]*8
loop_clr[0]=st7789.color565(255, 255, 255)
loop_clr[1]=st7789.color565(255, 0, 0)
loop_clr[2]=st7789.color565(255, 125, 0)
loop_clr[3]=st7789.color565(255, 255, 0)
loop_clr[4]=st7789.color565(0, 255, 0)
loop_clr[5]=st7789.color565(0, 255, 255)
loop_clr[6]=st7789.color565(0, 0, 255)
loop_clr[7]=st7789.color565(255, 0, 255)


display.fill(st7789.color565(255, 255, 255))

def to_euler_angles(data):
    Thre=2*(data[0]*data[2]-data[3]*data[1])
    while(Thre>=1 or Thre<=-1):
        data=imu.quat6()
        Thre=2*(data[0]*data[2]-data[3]*data[1])
    eu_w=data[0]
    eu_x=data[1]
    eu_y=data[2]
    eu_z=data[3]
    angles = [0,0,0]
    eu_r = math.atan2(2*(eu_w*eu_x+eu_y*eu_z),1-2*(eu_x*eu_x+eu_y*eu_y))
    cfg=0
    while(cfg==0):
        try:
            eu_p = math.asin(2*(eu_w*eu_y-eu_z*eu_x))
        except ValueError as e:
            print(2*(eu_w*eu_y-eu_z*eu_x))
            print("EV")
        except TypeError as e:
            print(2*(eu_w*eu_y-eu_z*eu_x))
            print("ET")
        except OSError as e:
            print("EO")
        else:
            cfg=1
    eu_e = math.atan2(2*(eu_w*eu_z+eu_x*eu_y),1-2*(eu_z*eu_z+eu_y*eu_y))
    angles[0] = eu_r*180/math.pi+180
    angles[1] = eu_p*180/math.pi+180
    angles[2] = eu_e*180/math.pi+180
    return angles

zero_x=120
zero_y=120
zero_z=0

x=[-70,-70,70,70,-70,-70,70,70]
y=[-70,-70,-70,-70,70,70,70,70]
z=[-20,20,20,-20,-20,20,20,-20]

xg=[0,1,2,3,4,5,6,7]
yg=[0,1,2,3,4,5,6,7]
zg=[0,1,2,3,4,5,6,7]

def rotx(xc,yc,zc,xp,yp,zp,Rx):
    a=[xp,yp,zp]
    b=[1,0,0]
    xpp=a[0]*b[0]+a[1]*b[1]+a[2]*b[2]
    b=[0,math.cos(Rx),-math.sin(Rx)]
    ypp=a[0]*b[0]+a[1]*b[1]+a[2]*b[2]
    b=[0,math.sin(Rx),math.cos(Rx)]
    zpp=a[0]*b[0]+a[1]*b[1]+a[2]*b[2]
    [xg,yg,zg]=[xpp+xc,ypp+yc,zpp+zc]
    return [xg,yg,zg]

def roty(xc,yc,zc,xp,yp,zp,Ry):
    a=[xp,yp,zp]
    b=[math.cos(Ry),0,math.sin(Ry)]
    xpp=a[0]*b[0]+a[1]*b[1]+a[2]*b[2]
    b=[0,1,0]
    ypp=a[0]*b[0]+a[1]*b[1]+a[2]*b[2]
    b=[-math.sin(Ry),0,math.cos(Ry)]
    zpp=a[0]*b[0]+a[1]*b[1]+a[2]*b[2]
    [xg,yg,zg]=[xpp+xc,ypp+yc,zpp+zc]
    return [xg,yg,zg]

def rotz(xc,yc,zc,xp,yp,zp,Rz):
    a=[xp,yp,zp]
    b=[math.cos(Rz),-math.sin(Rz),0]
    xpp=a[0]*b[0]+a[1]*b[1]+a[2]*b[2]

    b=[math.sin(Rz),math.cos(Rz),0]
    ypp=a[0]*b[0]+a[1]*b[1]+a[2]*b[2]

    b=[0,1,0]
    zpp=a[0]*b[0]+a[1]*b[1]+a[2]*b[2]
    [xg,yg,zg]=[xpp+xc,ypp+yc,zpp+zc]
    return [xg,yg,zg]


def plotbox(xg,yg,zg,color):
    if(color==backgroud_clr):
        for i in (0,1,2):
            display.line(math.floor(xg[i]), math.floor(yg[i]), math.floor(xg[i+1]), math.floor(yg[i+1]), color)
        display.line(math.floor(xg[0]), math.floor(yg[0]), math.floor(xg[3]), math.floor(yg[3]), color)

        for i in (4,5,6):
            display.line(math.floor(xg[i]), math.floor(yg[i]), math.floor(xg[i+1]), math.floor(yg[i+1]), color)
        display.line(math.floor(xg[7]), math.floor(yg[7]), math.floor(xg[4]), math.floor(yg[4]), color)

        for i in (0,1,2,3):
            display.line(math.floor(xg[i]), math.floor(yg[i]), math.floor(xg[i+4]), math.floor(yg[i+4]),color)
    else:
        c_count=3
        for i in (0,1,2):
            display.line(math.floor(xg[i]), math.floor(yg[i]), math.floor(xg[i+1]), math.floor(yg[i+1]), loop_clr[c_count])
            c_count=c_count+1
        display.line(math.floor(xg[0]), math.floor(yg[0]), math.floor(xg[3]), math.floor(yg[3]), loop_clr[c_count])

        c_count=0
        for i in (4,5,6):
            display.line(math.floor(xg[i]), math.floor(yg[i]), math.floor(xg[i+1]), math.floor(yg[i+1]), loop_clr[c_count])
            c_count=c_count+1
        display.line(math.floor(xg[7]), math.floor(yg[7]), math.floor(xg[4]), math.floor(yg[4]), loop_clr[c_count])
        c_count=c_count+1

        for i in (0,1,2,3):
            display.line(math.floor(xg[i]), math.floor(yg[i]), math.floor(xg[i+4]), math.floor(yg[i+4]), loop_clr[c_count])
            c_count=c_count+1

shift=to_euler_angles(imu.quat6())
display.draw_string(10, 105, "Waffle",size=5,color=st7789.color565(255, 128, 0))
display.draw_string(170, 95, "Nano",size=3,color=st7789.color565(255, 128, 0))

sign=0
while(utime.time()<=60):
    if(utime.time()<=10 and sign==0):
        sign=1
        display.fill(loop_clr[0])
        display.draw_string(10, 105, "Waffle",size=5,color=st7789.color565(255, 128, 0))
        display.draw_string(170, 95, "Nano",size=3,color=st7789.color565(255, 128, 0))
    if(utime.time()==13):
        display.fill(loop_clr[1])
    if(utime.time()==15):
        display.fill(loop_clr[4])
    if(utime.time()==17):
        display.fill(loop_clr[6])
    if(utime.time()==19):
        display.fill(loop_clr[1])
    if(utime.time()==30):
        display.fill(loop_clr[4])
    if(utime.time()==40):
        display.fill(loop_clr[6])
    if(utime.time()==50):
        display.fill(loop_clr[2])
    if(utime.time()==60):
        display.fill(backgroud_clr)
    print(utime.time())
    shift=to_euler_angles(imu.quat6())
display.fill(backgroud_clr)

while True:
    try:
        sign=0
        imu_data=imu.quat6()
        imu_data=imu.quat6()
        imu_data=imu.quat6()
        imu_data=imu.quat6()
        imu_data=imu.quat6()
        msg=to_euler_angles(imu_data)
    except ValueError as e:
        sign=0
    except TypeError as e:
        sign=0
    except OSError as e:
        sign=0
    else:
        sign=1
    if(sign==1):
        msg[0]=int((msg[0]-shift[0]+360)%360)
        msg[1]=int((msg[1]-shift[1]+360)%360)
        msg[2]=int((msg[2]-shift[2]+360)%360)
        print(msg)
        x=[-70,-70,70,70,-70,-70,70,70]
        y=[-70,-70,-70,-70,70,70,70,70]
        z=[-20,20,20,-20,-20,20,20,-20]
        rad0=(360-msg[0])*math.pi/180
        rad1=(msg[1])*math.pi/180
        rad2=(msg[2])*math.pi/180
        plotbox(xg,yg,zg,backgroud_clr)
        for i in (0,1,2,3,4,5,6,7):
            [xg[i],yg[i],zg[i]]=rotx(zero_x,zero_y,zero_z,x[i],y[i],z[i],rad0)
            [x[i],y[i],z[i]]=[xg[i]-zero_x,yg[i]-zero_y,zg[i]-zero_z]
        for i in (0,1,2,3,4,5,6,7):
            [xg[i],yg[i],zg[i]]=roty(zero_x,zero_y,zero_z,x[i],y[i],z[i],rad2)
            [x[i],y[i],z[i]]=[xg[i]-zero_x,yg[i]-zero_y,zg[i]-zero_z]
        for i in (0,1,2,3,4,5,6,7):
            [xg[i],yg[i],zg[i]]=rotz(zero_x,zero_y,zero_z,x[i],y[i],z[i],rad1)
            [x[i],y[i],z[i]]=[xg[i]-zero_x,yg[i]-zero_y,zg[i]-zero_z]
        plotbox(xg,yg,zg,obj_clr)
print("end")